#include "physics_manager.h"
#include "scene_manager.h"
#include "sfx_manager.h"

physics_manager::physics_manager()
: mx(0.0f)
, my(0.0f)
{
	cpInitChipmunk();
	space = cpSpaceNew();
	space->gravity = cpv(0, 980);
	mouseBody = cpBodyNew(INFINITY, INFINITY);
	mousePoint_last = cpv(0, 0);
	mouseJoint = NULL;
	cpSpaceAddCollisionHandler(space,COLLISION_TYPE_ENTITY,COLLISION_TYPE_ENTITY,NULL,body_collide_handle,NULL,NULL,NULL);
}

physics_manager::~physics_manager()
{

}

cpSpace* physics_manager::get_space()
{
    return space;
}

void physics_manager::update_physics_logic(float dt)
{
    cpSpaceStep(space, dt);
    update_mouse_body();
}

void physics_manager::update_mouse_body()
{
	hge->Input_GetMousePos(&mx, &my);
	cpVect newPoint = cpvlerp(mousePoint_last, cpv(mx, my), 0.25f);
	mouseBody->p = newPoint;
	mouseBody->v = cpvmult(cpvsub(newPoint, mousePoint_last), 120.0f);
	mousePoint_last = newPoint;
}

int physics_manager::body_collide_handle(cpArbiter *arb, struct cpSpace *space, void *data)
{
	cpBody* a;
	cpBody* b;
	cpArbiterGetBodies(arb, &a, &b);
	int punch = cpvlength(cpBodyGetVel(a)+cpBodyGetVel(b))/10;
	int pan = (((a->p.x + b->p.x)/2.0)/800)*200-100;
	sfx_manager::get_instance()->play_sound(SOUND_DANG, punch, pan);
	return 1;
}

void physics_manager::release_all_force()
{
    left_mouse_up();
}

bool physics_manager::shape_query(cpShape* s, float x, float y)
{
    cpShape *qs = cpSpacePointQueryFirst(space, cpv(x, y), GRABABLE_MASK_BIT, 0);
	return (s == qs);
}

void physics_manager::delete_entity_from_space(entity* e)
{
	for (size_t i=0; i<e->shapes.size(); ++i)
	{
        cpSpaceRemoveShape(space, e->shapes[i]);
	}
	cpSpaceRemoveBody(space, e->body);

	for (size_t i=0; i<e->shapes.size(); ++i)
	{
	    cpShapeFree(e->shapes[i]);
	}
	cpBodyFree(e->body);
}

void physics_manager::left_mouse_down()
{
	cpShape *shape = cpSpacePointQueryFirst(space, cpv(mx, my), GRABABLE_MASK_BIT, 0);
	if (shape)
	{
		cpBody *body = shape->body;
		mouseJoint = cpPivotJointNew2(mouseBody, body, cpvzero, cpBodyWorld2Local(body, cpv(mx, my)));
		mouseJoint->maxForce = 500000.0f;
		cpSpaceAddConstraint(space, mouseJoint);
	}
}

void physics_manager::left_mouse_up()
{
	if(mouseJoint)
	{
		cpSpaceRemoveConstraint(space, mouseJoint);
		cpConstraintFree(mouseJoint);
		mouseJoint = NULL;
	}
}

void physics_manager::add_entity_to_space(entity* e)
{
	cpSpaceAddBody(space, e->body);
	for (size_t i=0; i<e->shapes.size(); i++)
	{
		cpSpaceAddShape(space, e->shapes[i]);
	}
}

cpBody* physics_manager::make_body(float mass, float x, float y, float radius)
{
	cpBody* b = cpBodyNew(mass, cpMomentForCircle(mass, radius-1, radius, cpvzero));
	b->p = cpv(x, y);
	return b;
}

cpShape* physics_manager::make_circle_shape(float radius, float elasticity, float friction, cpBody* body)
{
	cpShape* s = cpCircleShapeNew(body, radius, cpvzero);
	s->e = elasticity;
	s->u = friction;
	s->data = body;
	s->collision_type = COLLISION_TYPE_ENTITY;
	return s;
}

cpShape* physics_manager::make_poly_shape(int vertex_count, cpVect* vertexs, float elasticity, float friction, cpBody* body)
{
	cpShape* s;
	s = cpPolyShapeNew(body, vertex_count, vertexs, cpvzero);
	s->e = elasticity;
	s->u = friction;
	s->data = body;
	s->collision_type = COLLISION_TYPE_ENTITY;

	//reset body moment of inertia
	cpBodySetMoment(body, cpMomentForPoly(body->m, vertex_count, vertexs, cpvzero));

	return s;
}

void physics_manager::set_screen_bound(cpVect lt, cpVect rb)
{
	//add wall thickness
    lt.x -= 20.0f;
	lt.y -= 20.0f;
	rb.x += 20.0f;
	rb.y += 20.0f;


	cpBody *staticBody;
	staticBody = cpBodyNew(INFINITY, INFINITY);
	cpShape* shape;
	//top
	shape = cpSegmentShapeNew(staticBody,cpv(lt.x, lt.y), cpv(rb.x, lt.y), 20.0f);
	shape->e = 1.0;
	shape->u = 1.0;
	shape->collision_type = COLLISION_TYPE_ENTITY;
	shape->layers = NOT_GRABABLE_MASK;
	cpSpaceAddStaticShape(space, shape);
	//bottom
	shape = cpSegmentShapeNew(staticBody,cpv(lt.x, rb.y), cpv(rb.x, rb.y), 20.0f);
	shape->e = 1.0;
	shape->u = 1.0;
	shape->collision_type = COLLISION_TYPE_ENTITY;
	shape->layers = NOT_GRABABLE_MASK;
	cpSpaceAddStaticShape(space, shape);
	//Left
	shape = cpSegmentShapeNew(staticBody, cpv(lt.x, lt.y), cpv(lt.x, rb.y), 20.0f);
	shape->e = 1.0; shape->u = 1.0;
	shape->collision_type = COLLISION_TYPE_ENTITY;
	shape->layers = NOT_GRABABLE_MASK;
	cpSpaceAddStaticShape(space, shape);
	//Right
	shape = cpSegmentShapeNew(staticBody,cpv(rb.x, lt.y), cpv(rb.x, rb.y), 20.0f);
	shape->e = 1.0; shape->u = 1.0;
	shape->collision_type = COLLISION_TYPE_ENTITY;
	shape->layers = NOT_GRABABLE_MASK;
	cpSpaceAddStaticShape(space, shape);
}